/*
 * @Description: 测试node，将订阅的点云信息直接作/current_scan发布出去
 * @Author: Sang Hao
 * @Date: 2021-09-19 15:36:07
 * @LastEditTime: 2021-10-27 13:54:19
 * @LastEditors: Sang Hao
 */

#include <ros/ros.h>
#include <pcl/common/transforms.h>

#include "glog/logging.h"
#include "lidar_slam/global_defination/global_defination.h"
#include "lidar_slam/subscriber/cloud_subscriber.hpp"
#include "lidar_slam/publisher/cloud_publisher.hpp"


using namespace lidar_slam;

int main(int argc, char * argv[]) {
	google::InitGoogleLogging(argv[0]);
	FLAGS_log_dir = WORK_SPACE_PATH + "/log";
	ros::init(argc, argv, "test_frame_node");
	ros::NodeHandle nh;

	std::shared_ptr<CloudSubscriber> cloud_sub_ptr = 
		std::make_shared<CloudSubscriber>(nh, "/orig_cloud", 10000);
	std::shared_ptr<CloudPublisher> cloud_pub_ptr = 
		std::make_shared<CloudPublisher>(nh, "/current_scan", "/map", 10000);

	std::deque<CloudData> cloud_data_buff;

	ros::Rate rate(100);

	while (ros::ok()) {
		ros::spinOnce();
		cloud_sub_ptr->ParseData(cloud_data_buff);
		while (cloud_data_buff.size() > 0) {
			CloudData cloud_data = cloud_data_buff.front();
			cloud_data_buff.pop_front();
			cloud_pub_ptr->Publish(cloud_data.cloud_ptr);
		}
	}

	rate.sleep();

	return 0;
	
}